#pragma config(Sensor, in1,    potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  sonar,          sensorSONAR_inch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex393, openLoop)
#pragma config(Motor,  port7,           servoMotor,    tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                                 - Moving Straight with Encoders -                                  *|
|*                                      ROBOTC on VEX 2.0 CORTEX                                      *|
|*                                                                                                    *|
|*  This program implements a self-straightening algorithm that provides a higher power level to the  *|
|*  motor that has traveled less, determined by comparing the values of the two encoders.             *|
|*  There is a 2 second pause at the beginning of the program.                                        *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the        *|
|*        "Squarebot" model, but may not be needed for all robot configurations.                      *|
|*    2)  Whichever encoder is being used for feedback should be cleared just before it starts        *|
|*        counting by using the "SensorValue(encoder) = 0;".  This helps ensure consistancy.          *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor   - Port 2    rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor   - Port 3    leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Digital - Port 1,2  rightEncoder        VEX Shaft Encoder     Right side                        *|
|*    Digital - Port 3,4  leftEncoder         VEX Shaft Encoder     Left side                         *|
\*----------------------------------------------------------------------------------------------------*/


//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
	wait1Msec(1500);							// Robot waits for 2000 milliseconds before executing program

	SensorValue[rightEncoder] = 0;	  // Set the encoder so that it starts counting at 0
	SensorValue[leftEncoder]  = 0;	  // Set the encoder so that it starts counting at 0

	while(SensorValue[rightEncoder]< 4220 || SensorValue[leftEncoder]< 4220)		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;		    // Right Motor is run at power level 50
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 50;		    // Left Motor is run at power level 50
		}
	}



	//END FIRST LENGTH, START TURN

	while(SensorValue[rightEncoder]> -365 || SensorValue[leftEncoder]< 365)
	{
	motor[leftMotor] = 100;
	motor[rightMotor] = -100;

	if(SensorValue[rightEncoder]< -365) {
			motor[rightMotor] = 0;
			}
if(SensorValue[leftEncoder]> 365) {
	motor[leftMotor] = 0;
			}
	}
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1500);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;

//END TURN

	//OPEN CLAW

	while(SensorValue[potentiometer]<2000)
{
motor[servoMotor] = -127;
}

motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1500);

	//START SECOND LENGTH
 while(SensorValue[rightEncoder]< 1700 || SensorValue[leftEncoder]< 1700)		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;
			motor[leftMotor]  = 100;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 100;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;
			motor[leftMotor]  = 50;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);


	//END OF SECOND LENGTH
//CLOSE CLAW

	while(SensorValue[potentiometer]>1500)
{
	motor[servoMotor] = 127;
}
SensorValue[rightEncoder] = 0;
SensorValue[leftEncoder] = 0;

motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);

//START SECOND TURN

	while(SensorValue[rightEncoder]> -410 || SensorValue[leftEncoder]< 410)
	{
	motor[leftMotor] = 100;
	motor[rightMotor] = -100;

	if(SensorValue[rightEncoder]< -410) {
			motor[rightMotor] = 0;
			}
if(SensorValue[leftEncoder]> 410) {
	motor[leftMotor] = 0;
			}
	}
  motor[rightMotor] = 0;
 motor[leftMotor] = 0;
 wait1Msec(1500);
	SensorValue[rightEncoder] = 0;
 SensorValue[leftEncoder] = 0;


//START THIRD LENGTH

 while(SensorValue[rightEncoder]< 2167 || SensorValue[leftEncoder]< 2167)		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;
			motor[leftMotor]  = 100;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 100;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;
			motor[leftMotor]  = 50;
		}
	}


	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;

	//END THIRD LENGTH
			//START THIRD TURN

		while(SensorValue[rightEncoder]> -390 || SensorValue[leftEncoder]< 390)
	{
	motor[leftMotor] = 100;
	motor[rightMotor] = -100;

	if(SensorValue[rightEncoder]< -390) {
			motor[rightMotor] = 0;
			}
if(SensorValue[leftEncoder]> 390) {
	motor[leftMotor] = 0;
			}
	}


motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1500);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;

//END TURN
	//START FOURTH LENGTH

	while(SensorValue[rightEncoder]< 2090 || SensorValue[leftEncoder]< 2090)		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;
			motor[leftMotor]  = 100;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 100;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;
			motor[leftMotor]  = 50;
		}
	}


	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;

	//END FOURTH LENGTH
	//OPEN CLAW

	while(SensorValue[potentiometer]<2100)
{
motor[servoMotor] = -127;
}

motor[rightMotor] = -60;
motor[leftMotor] = -60;
wait1Msec(150);

//END TASK

	}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
